#include <iostream>
#include <ros/ros.h>
#include <swarm_plan/UavsInfoStamped.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>

using namespace std;

geometry_msgs::PoseStamped uav_odom[4];
geometry_msgs::PoseStamped cam_pitch[4];
int ctrl_rate = 60;

void uav0_pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    uav_odom[0] = *msg;
}
void uav1_pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    uav_odom[1] = *msg;
}
void uav2_pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    uav_odom[2] = *msg;
}
void uav3_pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    uav_odom[3] = *msg;
}
void cam0_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    cam_pitch[0] = *msg;
}
void cam1_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    cam_pitch[1] = *msg;
}
void cam2_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    cam_pitch[2] = *msg;
}
void cam3_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    cam_pitch[3] = *msg;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "swarm_info_recv");
    ros::NodeHandle nh;
    ros::Rate ctrl_loop(ctrl_rate);

    ros::Publisher swarm_info_pub = nh.advertise<swarm_plan::UavsInfoStamped>("/swarm_plan/swarm_info", 1);

    ros::Subscriber uav0_pose_sub = nh.subscribe("/uav0/mavros/local_position/pose", 1, uav0_pose_cb);
    ros::Subscriber uav1_pose_sub = nh.subscribe("/uav1/mavros/local_position/pose", 1, uav1_pose_cb);
    ros::Subscriber uav2_pose_sub = nh.subscribe("/uav2/mavros/local_position/pose", 1, uav2_pose_cb);
    ros::Subscriber uav3_pose_sub = nh.subscribe("/uav3/mavros/local_position/pose", 1, uav3_pose_cb);
    ros::Subscriber cam0_sub = nh.subscribe("/iris_0/gimbal_yaw_angle", 1, cam0_cb);
    ros::Subscriber cam1_sub = nh.subscribe("/iris_1/gimbal_yaw_angle", 1, cam1_cb);
    ros::Subscriber cam2_sub = nh.subscribe("/iris_2/gimbal_yaw_angle", 1, cam2_cb);
    ros::Subscriber cam3_sub = nh.subscribe("/iris_3/gimbal_yaw_angle", 1, cam3_cb);

    ros::spinOnce();
    
    while(ros::ok()){
        swarm_plan::UavsInfoStamped swarminfo_msgs;
        swarminfo_msgs.header.stamp = ros::Time::now();
        for (int i = 0; i < 4;i++){
            swarminfo_msgs.poses[i] = uav_odom[i].pose.position;
            swarminfo_msgs.PitchOfCamera[i].data = cam_pitch[i].pose.position.x;
        }
        swarm_info_pub.publish(swarminfo_msgs);

        ros::spinOnce();
        ctrl_loop.sleep();
    }

    return 0;
}

